#include "AngleSolver.hpp"
#include "opencv2/opencv.hpp"
#include "math.h"

using namespace cv;
using namespace std;

namespace rm
{
double gravityOffset(double v, double theta, double _euclideanDistance)
{
    if(v<=0||v>30)
        v = 28;
    if(0<v&&v<15.5)    //步兵弹速分档,具体根据实际情况微调,可以问电控
    {
        v = 13.35;
    }
    if(15.5<v&&v<19.5)
    {
        v = 18;
    }
    if(19.5<v&&v<=30)
    {
        v = 26.4;
    }

    ///cout<<"theta:  "<<theta<<"dis:  "<<_euclideanDistance<<endl;
    theta=theta*CV_PI/180;

    double d=_euclideanDistance/100;  //对距离进行四舍五入,精确到分米位
    ///cout<<"d1   :"<<d;
    double temp = d-floor(d);
    if(temp>=0.5)
    {
        d=floor(d)+1;
    }
    if(temp<0.5)
    {
        d=floor(d);
    }

    double euclideanDistance=d/10;

    double g=9.8;
    double x=euclideanDistance*cos(theta);
    double y=euclideanDistance*sin(theta);
    ///cout<<"x:"<<x<<"y:"<<y<<"d:"<<d<<endl;
    double _theta=atan(-(v*v/g/x/x)*(-x+sqrt(x*x-2*(g*x*x/v/v)*(g*x*x/2/v/v+y))));//抛物线弹道模型

    _theta=_theta*180/CV_PI;
    theta=theta*180/CV_PI;

    if(x*x-2*(g*x*x/v/v)*(g*x*x/2/v/v+y)<=0)
    {
        return 0;
    }
    double h = 0.04;    //调参
    if(v <=15.1)
    {
        h = 0;
    }
    //cout<<":::::::::::::::::"<<(180/CV_PI)*(atan(h/2)-atan(h/euclideanDistance))<<endl;
    return _theta-theta-(180/CV_PI)*(atan(h/2)-atan(h/euclideanDistance));//由标定和校准方法引起的系统误差
}

void AngleSolver::initPredictor()
{
    history_ErrAngle=0;//history_ErrAngle[0]=0,history_ErrAngle[1]=0;
    history_PreAngle=0;//[0]=0,history_PreAngle[1]=0;
    history_PrePreAngle=0;//[0]=0,history_PrePreAngle[1]=0;
    history_PrePrePreAngle=0;//[0]=0,history_PrePrePreAngle[1]=0;
}

const cv::Vec2f AngleSolver::predict(cv::Vec2f now_ErrAngle,double timeStamp)
{
    //if(!timeStamp)//move while lost
    //    {
    //        cout<<"armor is lost:    ";
    //        if(abs(history_ErrAngle[0])>8)//try to change here if it is too easy to stop after lost
    //        {
    //            history_ErrAngle[0]=history_ErrAngle[0]/10;//slow down
    //            cout<<"slow down: down to history_Errangle[0]="<<history_ErrAngle<<endl;
    //            return history_ErrAngle;
    //        }
    //        else//keep the speed
    //        {
    //            cv::Vec2f predict_angle;
    //            predict_angle=(history_ErrAngle+history_PrePreAngle)+
    //                    ((history_ErrAngle+history_PrePreAngle)-(history_ErrAngle+history_PrePrePreAngle))*2;

    //            history_ErrAngle=history_ErrAngle;
    //            history_PrePrePreAngle=history_PrePreAngle;
    //            history_PrePreAngle=history_PreAngle;
    //            history_PreAngle=predict_angle;
    //            cout<<"keep the speed: now_ErrAngle[0]="<<now_ErrAngle[0]<<"  predict_angle[0]="<<predict_angle<<endl;
    //            return history_ErrAngle+7*history_PreAngle+history_PrePreAngle;
    //        }
    //    }
    //else
    if(abs(now_ErrAngle[0]) >= 10)
    {
        history_ErrAngle=now_ErrAngle[0];
        history_PreAngle=now_ErrAngle[0];
        history_PrePreAngle=0,history_PrePreAngle=0;
        history_PrePrePreAngle=0,history_PrePrePreAngle=0;
        cout<<"Changing the target: now_ErrAngle[0]="<<now_ErrAngle[0]<<"  predict_angle[0]="<<now_ErrAngle<<endl;
        return now_ErrAngle;
    }
    else
        if(!history_ErrAngle)//waiting for new digiitals
        {
            history_ErrAngle=now_ErrAngle[0];
            history_PrePrePreAngle=history_PrePreAngle;
            history_PrePreAngle=history_PreAngle;
            history_PreAngle=now_ErrAngle[0];
            cout<<"Starting the predictor: now_ErrAngle[0]="<<now_ErrAngle[0]<<"  predict_angle[0]="<<now_ErrAngle<<endl;
            return now_ErrAngle;
        }
    //        else if(abs(history_ErrAngle[0])+abs(history_PreAngle[0])+abs(history_PrePreAngle[0])<0.5)
    //        {4
    //            cv::Vec2f predict_angle;
    //            predict_angle=(now_ErrAngle+history_PrePreAngle)+
    //                    ((now_ErrAngle+history_PrePreAngle)-(history_ErrAngle+history_PrePrePreAngle))*2;

    //            history_ErrAngle=now_ErrAngle;
    //            history_PrePrePreAngle=history_PrePreAngle;
    //            history_PrePreAngle=history_PreAngle;
    //            history_PreAngle=predict_angle;
    //            cout<<"Moving from static: now_ErrAngle[0]="<<now_ErrAngle[0]<<"  predict_angle[0]="<<predict_angle<<endl;
    //            return now_ErrAngle+7*history_PreAngle+history_PrePreAngle;//added speed to increase the speed
    //        }
        else
        {
            float predictAngle;
            predictAngle=(now_ErrAngle[0]+history_PrePreAngle)+
                    ((now_ErrAngle[0]+history_PrePreAngle)-(history_ErrAngle+history_PrePrePreAngle))*2;
            history_ErrAngle=now_ErrAngle[0];
            history_PrePrePreAngle=history_PrePreAngle;
            history_PrePreAngle=history_PreAngle;
            history_PreAngle=predictAngle;
            //        history_PreAngle=predict_angle;
            //        history_ErrAngle[0]=now_ErrAngle[0];
            //        history_PrePrePreAngle[0]=history_PrePreAngle[0];
            //        history_PrePreAngle[0]=history_PreAngle[0];
            //        history_PreAngle[0]=predict_angle;
            cout<<"Normal predicting: now_ErrAngle[0]="<<now_ErrAngle[0]<<"  predict_angle[0]="<<history_PreAngle<<endl;
            return cv::Vec2f(now_ErrAngle[0]+history_PreAngle+history_PrePrePreAngle,now_ErrAngle[1]);
        }
}

/*
*      z
*     /
*  O /______x
*    |
*    |
*    y
*/

std::vector<cv::Point3f> AngleSolverParam::POINT_3D_OF_ARMOR_BIG = std::vector<cv::Point3f>
{
        cv::Point3f(-115, -28, 0),	//tl
        cv::Point3f(115, -28, 0),	//tr
        cv::Point3f(115, 28, 0),	//br
        cv::Point3f(-115, 28, 0)	//bl
};std::vector<cv::Point3f> AngleSolverParam::POINT_3D_OF_ARMOR_SMALL = std::vector<cv::Point3f>
{
        cv::Point3f(-67.5, -28, 0),	//tl
        cv::Point3f(67.5, -28, 0),	//tr
        cv::Point3f(67.5, 28, 0),		//br
        cv::Point3f(-67.5, 28, 0)		//bl
};

// std::vector<cv::Point3f> AngleSolverParam::POINT_3D_OF_RUNE = std::vector<cv::Point3f>
//	{
//		cv::Point3f(-370, -220, 0),
//		cv::Point3f(0, -220, 0),
//		cv::Point3f(370, -220, 0),
//		cv::Point3f(-370, 0, 0),
//		cv::Point3f(0, 0, 0),
//		cv::Point3f(370, 0, 0),
//		cv::Point3f(-370, 220, 0),
//		cv::Point3f(0, 220, 0),
//		cv::Point3f(370, 220, 0)
//	};
// std::vector<cv::Point3f> AngleSolverParam::POINT_3D_OF_RUNE = std::vector<cv::Point3f>
//    {
//         cv::Point3f(-103, -47, 0),	//tl
//         cv::Point3f(103, -47, 0),	//tr
//         cv::Point3f(103, 47, 0),	//br
//         cv::Point3f(-103, 47, 0)	//bl
//    };

std::vector<cv::Point3f> AngleSolverParam::POINT_3D_OF_RUNE = std::vector<cv::Point3f>
{
        cv::Point3f(-76, -35, 0),	//tl
        cv::Point3f(76, -35, 0),	//tr
        cv::Point3f(76, 35, 0),	//br
        cv::Point3f(-76, 35, 0)	//bl
};

AngleSolver::AngleSolver()
{
    for (int ll = 0; ll <= 3; ll++)
        target_nothing.push_back(cv::Point2f(0.0, 0.0));
}

AngleSolver::AngleSolver(const AngleSolverParam & AngleSolverParam)
{
    _params = AngleSolverParam;
    _cam_instant_matrix = _params.CAM_MATRIX.clone();
    for (int ll = 0; ll <= 3; ll++)
        target_nothing.push_back(cv::Point2f(0.0, 0.0));
}

void AngleSolver::init(const AngleSolverParam& AngleSolverParam)
{
    _params = AngleSolverParam;
    _cam_instant_matrix = _params.CAM_MATRIX.clone();
}

void AngleSolver::setTarget(const std::vector<cv::Point2f> objectPoints, int objectType)
{
    if (objectType == 0 || objectType == 1)
    {
        if (angle_solver_algorithm == 0 || angle_solver_algorithm == 2) {
            angle_solver_algorithm = 1; cout << "algorithm is reset to PNP Solution" << endl;
        }
        point_2d_of_armor = objectPoints;
        if (objectType == 0)
            enemy_type = 0;
        else
            enemy_type = 1;
        return;
    }
    if (objectType == 3 || objectType == 4)
    {
        angle_solver_algorithm = 2;
        point_2d_of_rune = objectPoints;
    }

}

void AngleSolver::setTarget(const std::vector<cv::Point2f> objectPoints,const cv::Point2f Center_of_armor, int objectType)
{
    if (objectType == 0 || objectType == 1 || objectType == 2)
    {
        if (angle_solver_algorithm == 0 || angle_solver_algorithm == 1) {
            angle_solver_algorithm = 2; cout << "algorithm is reset to PNP & One Point Solution" << endl;
        }
        point_2d_of_armor = objectPoints;
        centerPoint = Center_of_armor;
        enemy_type = objectType;
        return;
    }
    if (objectType == 3 || objectType == 4)
    {
        angle_solver_algorithm = 2;
        point_2d_of_rune = objectPoints;
    }
}

void AngleSolver::setTarget(const cv::Point2f Center_of_armor, int objectPoint)
{
    if (angle_solver_algorithm == 1 || angle_solver_algorithm == 2) {
        angle_solver_algorithm = 0; cout << "algorithm is reset to One Point Solution" << endl;
    }
    centerPoint = Center_of_armor;
    if (objectPoint == 3 || objectPoint == 4)
        is_shooting_rune = 1;
    else {
        is_shooting_rune = 0;
        _rune_compensated_angle = 0;
    }
}


#ifdef DEBUG

void AngleSolver::showPoints2dOfArmor()
{
    cout << "the point 2D of armor is" << point_2d_of_armor << endl;
}


void AngleSolver::showTvec()
{
    cv::Mat tvect;
    transpose(_tVec, tvect);
    cout << "the current _tVec is:" << endl << tvect << endl;
}

void AngleSolver::showEDistance()
{
    cout << "  _euclideanDistance is  " << _euclideanDistance / 1000 << "m" << endl;
}

void AngleSolver::showcenter_of_armor()
{
    cout << "the center of armor is" << centerPoint << endl;
}

void AngleSolver::showAngle()
{
    cout << "_xErr is  " << _xErr << "  _yErr is  " << _yErr << endl;
}

int AngleSolver::showAlgorithm()
{
    return angle_solver_algorithm;
}
#endif // DEBUG


AngleSolver::AngleFlag AngleSolver::solve()
{
    if (angle_solver_algorithm == 1)
    {
        if (enemy_type == 1)
            solvePnP(_params.POINT_3D_OF_ARMOR_BIG, point_2d_of_armor, _cam_instant_matrix, _params.DISTORTION_COEFF, _rVec, _tVec, false, CV_ITERATIVE);
        if (enemy_type == 0)
            solvePnP(_params.POINT_3D_OF_ARMOR_SMALL, point_2d_of_armor, _cam_instant_matrix, _params.DISTORTION_COEFF, _rVec, _tVec, false, CV_ITERATIVE);
        _tVec.at<double>(1, 0) -= _params.Y_DISTANCE_BETWEEN_GUN_AND_CAM;
        _xErr = atan(_tVec.at<double>(0, 0) / _tVec.at<double>(2, 0)) / 2 / CV_PI * 360;
        _yErr = atan(_tVec.at<double>(1, 0) / _tVec.at<double>(2, 0)) / 2 / CV_PI * 360;
        _euclideanDistance = sqrt(_tVec.at<double>(0, 0)*_tVec.at<double>(0, 0) + _tVec.at<double>(1, 0)*_tVec.at<double>(1, 0) + _tVec.at<double>(2, 0)* _tVec.at<double>(2, 0));

        Mat R;
        Rodrigues(_rVec, R);
        Mat k = (Mat_<double>(3, 1) << 0, 0, 1);  //世界坐标系下的(0, 0, 1)向量
        Mat rot_k = (Mat_<double>(3, 1) << 0, 0, 1);    //相机坐标里k的坐标。相机坐标系到世界坐标系有旋转。如果不考虑坐标系，两个k是平行的
        rot_k = R*k;
        target_direction = Point3f(rot_k.at<double>(0, 0), rot_k.at<double>(1, 0), rot_k.at<double>(2, 0));

        if (_euclideanDistance >= 9500) {
            return TOO_FAR;
        }
        return ANGLES_AND_DISTANCE;
    }


    if (angle_solver_algorithm == 0)
    {
        double x1, x2, y1, y2, r2, k1, k2, p1, p2, y_ture;
        x1 = (centerPoint.x - _cam_instant_matrix.at<double>(0, 2)) / _cam_instant_matrix.at<double>(0, 0);
        y1 = (centerPoint.y - _cam_instant_matrix.at<double>(1, 2)) / _cam_instant_matrix.at<double>(1, 1);

        r2 = x1 * x1 + y1 * y1;
        k1 = _params.DISTORTION_COEFF.at<double>(0, 0);
        k2 = _params.DISTORTION_COEFF.at<double>(1, 0);
        p1 = _params.DISTORTION_COEFF.at<double>(2, 0);
        p2 = _params.DISTORTION_COEFF.at<double>(3, 0);
        x2 = x1 * (1 + k1 * r2 + k2 * r2*r2) + 2 * p1*x1*y1 + p2 * (r2 + 2 * x1*x1);
        y2 = y1 * (1 + k1 * r2 + k2 * r2*r2) + 2 * p2*x1*y1 + p1 * (r2 + 2 * y1*y1);
        _xErr = atan(x2) / 2 / CV_PI * 360;
        _yErr = atan(y2) / 2 / CV_PI * 360;
        if (is_shooting_rune) _yErr -= _rune_compensated_angle;
        return ONLY_ANGLES;
    }


    if (angle_solver_algorithm == 2)
    {
        if (enemy_type == 1){
            cout<<"big"<<endl;
            solvePnP(_params.POINT_3D_OF_ARMOR_BIG, point_2d_of_armor, _cam_instant_matrix, _params.DISTORTION_COEFF, _rVec, _tVec, false, CV_ITERATIVE);
        }
        if (enemy_type == 0){
            cout<<"small"<<endl;
            solvePnP(_params.POINT_3D_OF_ARMOR_SMALL, point_2d_of_armor, _cam_instant_matrix, _params.DISTORTION_COEFF, _rVec, _tVec, false, CV_ITERATIVE);
        }
        if (enemy_type == 2){
            cout<<"windmill"<<endl;
            solvePnP(_params.POINT_3D_OF_RUNE, point_2d_of_armor, _cam_instant_matrix, _params.DISTORTION_COEFF, _rVec, _tVec, false, CV_ITERATIVE);
        }
        _tVec.at<double>(1, 0) -= _params.Y_DISTANCE_BETWEEN_GUN_AND_CAM;

        if(_euclideanDistance != 0 && _xErr != 0 && _yErr != 0)
        {
            pre_euclideanDistance = _euclideanDistance;
            pre_xErr = _xErr;
            pre_yErr = _yErr;
        }

        _euclideanDistance = sqrt(_tVec.at<double>(0, 0)*_tVec.at<double>(0, 0) + _tVec.at<double>(1, 0)*_tVec.at<double>(1, 0) + _tVec.at<double>(2, 0)* _tVec.at<double>(2, 0));
        if (_euclideanDistance >= 10000) {
            return TOO_FAR;
        }

        Mat R;
        Rodrigues(_rVec, R);
        Mat k = (Mat_<double>(3, 1) << 0, 0, 1);  //世界坐标系下的(0, 0, 1)向量
        Mat rot_k = (Mat_<double>(3, 1) << 0, 0, 1);    //相机坐标里k的坐标。相机坐标系到世界坐标系有旋转。如果不考虑坐标系，两个k是平行的
        rot_k = R*k;
        target_direction = Point3f(rot_k.at<double>(0, 0), rot_k.at<double>(1, 0), rot_k.at<double>(2, 0)); //该方向从装甲板指向远离摄像头的方向，一般来说是指向车的内部
        cout<<"aaaaaaaaaaa"<<acos(target_direction.x)*180/CV_PI<<endl;

        double x, y;
        x = centerPoint.x;
        y = centerPoint.y;
        ///cout<<"xobj..."<<x<<"yobj..."<<y<<endl;
        double fx=_cam_instant_matrix.at<double>(0,0);
        double fy=_cam_instant_matrix.at<double>(1,1);
        double cx=_cam_instant_matrix.at<double>(0,2);
        double cy=_cam_instant_matrix.at<double>(1,2);
        Point2f pnt;
        vector<cv::Point2f> in;
        vector<cv::Point2f> out;
        in.push_back(Point2f(x,y));
        //对像素点去畸变
        undistortPoints(in,out,_cam_instant_matrix,_params.DISTORTION_COEFF,noArray(),_cam_instant_matrix);
        pnt=out.front();

        //去畸变后的比值
        double rxNew=(pnt.x-cx)/fx;
        double ryNew=(pnt.y-cy)/fy;

        _xErr=atan(rxNew)/CV_PI*180;
        _yErr=atan(ryNew)/CV_PI*180;


        return ANGLES_AND_DISTANCE;
    }
    return ANGLE_ERROR;
}


void AngleSolver::setResolution(const cv::Size2i& image_resolution)
{
    image_size = image_resolution;
    //    int width = 1280;
    //    int height = 720;

    int width = image_resolution.width;
    int height = image_resolution.height;

    _cam_instant_matrix.at<double>(0, 2) = _params.CAM_MATRIX.at<double>(0, 2) -(width / 2 - image_size.width / 2);
    _cam_instant_matrix.at<double>(1, 2) = _params.CAM_MATRIX.at<double>(1, 2) -(height / 2 - image_size.height / 2);
    _cam_instant_matrix.at<double>(0, 0) = _params.CAM_MATRIX.at<double>(0, 0) /(height/image_size.height);
    _cam_instant_matrix.at<double>(1, 1) = _params.CAM_MATRIX.at<double>(1, 1) /(height/image_size.height);
}

void AngleSolver::setUserType(int usertype)
{
    user_type = usertype;
}

void AngleSolver::setEnemyType(int enemytype)
{
    enemy_type = enemytype;
}


void AngleSolver::setBulletSpeed(int bulletSpeed)
{
    _bullet_speed = bulletSpeed;
}

const cv::Vec2f AngleSolver::getAngle()
{
    return cv::Vec2f(_xErr, _yErr);
}


double AngleSolver::getDistance()
{
    return _euclideanDistance;
}

const cv::Point3f AngleSolver::getDirection()
{
    return target_direction;
}


double AngleSolver::KF_Distance()
{
    bool isrefresh = 0;
    if(pre_xErr != 0 && pre_yErr != 0 && pre_euclideanDistance != 0)
    {
        if((abs(_xErr-pre_xErr)>=5 || abs(_yErr-pre_yErr)>=3) && abs(_euclideanDistance-pre_euclideanDistance)>=500)
        {
            KF_Refresh();
            isrefresh = 1;
        }
    }

    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    KFP_distance.Now_P = KFP_distance.LastP + KFP_distance.Q;
    //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    KFP_distance.Kg = KFP_distance.Now_P / (KFP_distance.Now_P + KFP_distance.R);
    //假设距离呈线性变化
    if(isrefresh)
    {
        KFP_distance.out = _euclideanDistance;
    }
    else
    {
        KFP_distance.out += _euclideanDistance-pre_euclideanDistance;
    }
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    KFP_distance.out = KFP_distance.out + KFP_distance.Kg * (_euclideanDistance - KFP_distance.out);
    //更新协方差方程: 本次的系统协方差付给 kfp->LastP 为下一次运算准备。
    KFP_distance.LastP = (1-KFP_distance.Kg) * KFP_distance.Now_P;
    return KFP_distance.out;
}

void AngleSolver::KF_Refresh()
{
    KFP_distance = {0.02, 0, 0, 0, 0.001, 0.543};
}

void AngleSolverParam::readFile(const int id)
{
    cv::FileStorage fsread("/home/nuc/Robomaster2022/Pose/angle_solver_params.xml", cv::FileStorage::READ);
    if (!fsread.isOpened())
    {
        std::cerr << "failed to open xml" << std::endl;
        return;
    }
    fsread["Y_DISTANCE_BETWEEN_GUN_AND_CAM"] >> Y_DISTANCE_BETWEEN_GUN_AND_CAM;

#ifdef DEBUG 
    std::cout << Y_DISTANCE_BETWEEN_GUN_AND_CAM << std::endl;
#endif // DEBUG

    switch (id)
    {
    case 0:
    {
        fsread["CAMERA_MARTRIX_0"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_0"] >> DISTORTION_COEFF;
        return;
    }
    case 1:
    {
        fsread["CAMERA_MARTRIX_1"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_1"] >> DISTORTION_COEFF;
        return;
    }
    case 2:
    {
        fsread["CAMERA_MARTRIX_2"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_2"] >> DISTORTION_COEFF;
        return;
    }
    case 3:
    {
        fsread["CAMERA_MARTRIX_3"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_3"] >> DISTORTION_COEFF;
        return;
    }
    case 4:
    {
        fsread["CAMERA_MARTRIX_4"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_4"] >> DISTORTION_COEFF;
        return;
    }
    case 5:
    {
        fsread["CAMERA_MARTRIX_OFFICIAL"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_OFFICIAL"] >> DISTORTION_COEFF;
        return;
    }
    case 6:
    {
        fsread["CAMERA_MARTRIX_6"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_6"] >> DISTORTION_COEFF;
        return;
    }
    case 7:
    {
        fsread["CAMERA_MARTRIX_7"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_7"] >> DISTORTION_COEFF;
        return;
    }
    case 8:
    {
        fsread["CAMERA_MARTRIX_8"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_8"] >> DISTORTION_COEFF;
        return;
    }
    case 9:
    {
        fsread["CAMERA_MARTRIX_9"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_9"] >> DISTORTION_COEFF;
        return;
    }
    case 10:
    {
        fsread["CAMERA_MARTRIX_10"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_10"] >> DISTORTION_COEFF;
        return;
    }
    case 11:
    {
        fsread["CAMERA_MARTRIX_11"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_11"] >> DISTORTION_COEFF;
        return;
    }
    case 12:
    {
        fsread["CAMERA_MARTRIX_12"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_12"] >> DISTORTION_COEFF;
        return;
    }
    case 13:
    {
        fsread["CAMERA_MARTRIX_13"] >> CAM_MATRIX;
        fsread["DISTORTION_COEFF_13"] >> DISTORTION_COEFF;
        return;
    }
    default:
        std::cout << "wrong cam number given." << std::endl;
        return;
    }
}
}

